/*
  Copyright (c) 2010 Les Newell. All rights reserved

  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program. If not, see <http://www.gnu.org/licenses/>.
*/


/*
Matrix based kinematics module. For most types of rotary wing craft.
*/
#include "kinematics.h"

DEFINE_MODULE(MatrixKinematics, Kinematics)
//Note the lack of an opening curly bracket.

    //Only use this to expose sockets and parameters. Do most of your
    //initialisastion in Init()
    MatrixKinematics()
    {
    }

    //A parameter has changed. This happens at bootup if the EEPROM
    //contains valid data and at run-time if the user changes a
    //parameter over the serial link
    virtual void ParamChanged(Socket * param)
    {
    }

    //Initialise the module
    //set up hardware etc
    virtual void Init()
    {
        Kinematics::Init();
        ConnectPlugArray(matrixServos,m_outputs,MATRIX_OUTPUTS);

/*        for (int ct=0; ct< MATRIX_OUTPUTS; ct++)
        {
            m_outputs[ct].Connect(matrixServos[ct]);
        }*/
        SetInterval(MATRIX_INTERVAL);
        SetPriority(30);
    }

    //Here is where you do your work. It is called at the rate defined by SetInterval
    virtual void Loop(const unsigned long& interval)
    {

float f=0;
m_outputs[0] = f;
        if (m_flightMode < FLIGHTMODE_ACRO)
        {
            for (int ct = 0; ct < MATRIX_OUTPUTS; ct++)
            {
                m_outputs[ct] = motorDisarmed[ct];
            }
            return;
        }

        for (int output = 0; output < MATRIX_OUTPUTS; output++)
        {
            float result = 0;
            const float* row = matrixGains[output];
            for (int c=0; c< MAX_AXES; c++)
            {
                result += row[c] * m_commands[c];
            }
            result += (row[THROTTLE] * m_commands[THROTTLE] * 2) + row[MAX_COMMANDS];
            m_outputs[output] = result;
        }
    }

protected:
    PlugF m_outputs[MATRIX_OUTPUTS];
};

MatrixKinematics g_MatrixKinematics;
